/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Qik 2s9v1 Motor Controller Driver
 *         $Id: motor.c 5 2013-12-18 04:48:03Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "car.h"
#include "motor.h"

#define UART1_BAUD      38400
#define UART1_PR_VALUE  (uint16_t)(((double)0x200000 * UART1_BAUD) / DEFAULT_SYS_CLK_FREQ)

#define TIMEOUT_VALUE   1000

/****************************************************************************/
void uart1_putchar(uint8_t byte)
{
  while(!(SCON1 & 2));  // wait for TI flag
  SCON1 &= ~2;    // clear TI flag
  SBUF1 = byte;   // write byte to data register
}

/****************************************************************************/
uint8_t uart1_getchar(void)
{
  while (!(SCON1 & 1)); // Wait for receive complete flag to be set
  SCON1 &= ~1;          // Clear receive complete flag
  return SBUF1;
}

/****************************************************************************/
#define uart1_checkchar() (SCON1 & 1)

/****************************************************************************/
int motor_init(void)
{
  int timeout;
  uint8_t data;

  // Configure UART1 to talk to Motor Controller
  SMD1 = 0x02;         // Set baud rate select bit, no interrupt
  SCON1 = 0x50;        // Set mode 1 and receive enable for UART 0
  PR1 = UART1_PR_VALUE; // Set baud rate using phase register
  SCON1 |= 2; // set TI flag

  // Configure Motor Controller
  uart1_putchar(0x81);

  // Try to read the firmware vesion to detect if the controller is connected
  timeout = TIMEOUT_VALUE;
  while (!uart1_checkchar()) {
    if (--timeout <= 0) {
      maxq_printf(C"Timed out waiting for motor controller\r\n");
      return 0;
    }
  }

  // Check that the response if valid
  if ((data = uart1_getchar()) != '2') {
    maxq_printf(C"Invalid firmware version 0x%02x\r\n", data);
    return 0;
  }

#if 0  
  uart1_putchar(0x83);
  uart1_putchar(0x00);
  data = uart1_getchar();
  maxq_printf(C"Parameter 0 0x%02x\r\n", data);

  uart1_putchar(0x83);
  uart1_putchar(0x01);
  data = uart1_getchar();
  maxq_printf(C"Parameter 1 0x%02x\r\n", data);

  uart1_putchar(0x83);
  uart1_putchar(0x02);
  data = uart1_getchar();
  maxq_printf(C"Parameter 2 0x%02x\r\n", data);

  uart1_putchar(0x83);
  uart1_putchar(0x03);
  data = uart1_getchar();
  maxq_printf(C"Parameter 3 0x%02x\r\n", data);
#endif

  // Set PWM to high frequency 8-bit mode
  uart1_putchar(0x84);
  uart1_putchar(0x01);
  uart1_putchar(0x01);
  uart1_putchar(0x55);
  uart1_putchar(0x2A);

  // Set timeout to 262ms
  uart1_putchar(0x84);
  uart1_putchar(0x03);
  uart1_putchar(0x01);
  uart1_putchar(0x55);
  uart1_putchar(0x2A);

  // Read the error byte to clear any errors
  motor_error();

  return 1;
}

/****************************************************************************/
void motor_set(unsigned int wheel, unsigned int dir, uint8_t value)
{
  uint8_t cmd;

//maxq_printf(C"Set motor %u to 0x%02x\r\n", wheel, value);

  if (wheel == LEFT)
    cmd = 0x88;
  else
    cmd = 0x8C;

  if (dir == REVERSE)
    cmd += 2;

  if (value >= 0x80)
    cmd += 1;

  uart1_putchar(cmd);

  value &= 0x7F;
  uart1_putchar(value);
}

/****************************************************************************/
void motor_stop(unsigned int wheel)
{
  if (wheel == LEFT)
    uart1_putchar(0x88);
  else
    uart1_putchar(0x8C);
  uart1_putchar(0);
}

/****************************************************************************/
void motor_coast(unsigned int wheel)
{
//maxq_printf(C"Set motor %u to coast\r\n", wheel);

  if (wheel == LEFT)
    uart1_putchar(0x86);
  else
    uart1_putchar(0x87);
}

/****************************************************************************/
uint8_t motor_error(void)
{
  int timeout, retry;

  for (retry = 0; retry < 2; retry++) {
    uart1_putchar(0x82);

    for (timeout = 0; timeout < TIMEOUT_VALUE; timeout++) {
      if (uart1_checkchar())
        return uart1_getchar();
    }
  }

  maxq_printf(C"Timed out waiting for motor controller\r\n");
  return 0xFF;
}
